#include "Dijkstra.h"
#include <queue>

using namespace std;


//新建邻接表
void Dijkstra::Init() {
    //m_graph结构：<起点，<<终点，权值>,<终点，权值>,<终点，权值>>>
    QList<DataBase::edge> datas = DataBase::getInstance().getEdgeData();
    for(auto data:datas){
        m_graph[data.start].append(qMakePair(data.end, data.distance));
        m_graph[data.end].append(qMakePair(data.start, data.distance));
        // 确保目标节点存在于图中
        if (!m_graph.contains(data.end)) {
            m_graph[data.end] = QList<QPair<int, double>>();
        }
        if (!m_graph.contains(data.start)) {
            m_graph[data.start] = QList<QPair<int, double>>();
        }

    }
    //存储每一个节点的坐标
    Points=DataBase::getInstance().getPointData();
}

/*
 * 在邻接表的中添加当前位置的节点
 * position：当前位置
*/
// int Dijkstra::addPoint(QVector<double> position){
//     double t = 15;
//     QVector<QVector<int>> ans;
//     for(auto it=this->m_graph.begin();it!=m_graph.end();it++){
//         QVector<double> p=this->Points[it.key()-1];
//         //计算当前位置与当前节点的斜率
//         float target=(position[2]-p[2])/(position[0]-p[0]);
//         for(auto it1=it.value().begin();it1!=it.value().end();it1++){
//             QVector<double> q=this->Points[it1->first-1];
//             //计算当前节点与剩余所有节点的斜率
//             float k=(q[2]-p[2])/(q[0]-p[0]);
//             //若两者斜率小于一定阈值，则将该边加入ans
//             if (k-target<1e-2){
//                 QVector<int> temp;
//                 temp.push_back(it.key());
//                 temp.push_back(it1->first);
//                 ans.append(temp);
//             }
//         }
//     }

//     //输出加边信息
//     // qDebug()<<"ans:"<<ans;
//     // qDebug();
//     for(int i=0;i<ans.size();i++){
//         int res=0;
//         if (Points[ans[i][0]-1][0]<=position[0]+t || Points[ans[i][1]-1][0]<=position[0]+t)
//             res++;
//         if (Points[ans[i][0]-1][0]>=position[0]-t || Points[ans[i][1]-1][0]>=position[0]-t)
//             res++;
//         if(Points[ans[i][0]-1][2]<=position[2]+t || Points[ans[i][1]-1][2]<=position[2]+t)
//             res++;
//         if(Points[ans[i][0]-1][2]>=position[2]-t || Points[ans[i][1]-1][2]>=position[2]-t)
//             res++;
//         if (res==4){
//             Points.append(position);
//             double d1=qSqrt(pow((position[0]-Points[ans[i][0]-1][0]),2)+pow((position[2]-Points[ans[i][0]-1][2]),2));
//             double d2=qSqrt(pow((position[0]-Points[ans[i][1]-1][0]),2)+pow((position[2]-Points[ans[i][1]-1][2]),2));
//             for (int j=0;j<this->m_graph[ans[i][0]].size();j++){
//                 if(m_graph[ans[i][0]][j].first==ans[i][1]){
//                     m_graph[ans[i][0]][j].first=Points.size();
//                     m_graph[ans[i][0]][j].second=d1;
//                 }
//             }
//             for (int j=0;j<this->m_graph[ans[i][1]].size();j++){
//                 if(m_graph[ans[i][1]][j].first==ans[i][0]){
//                     m_graph[ans[i][1]][j].first=Points.size();
//                     m_graph[ans[i][1]][j].second=d2;
//                 }
//             }

//             m_graph[Points.size()].append(qMakePair(ans[i][0], d1));
//             m_graph[Points.size()].append(qMakePair(ans[i][1], d2));
//             //输出最后选择点以及加的两条边
//             // qDebug()<<ans[i][0]<<":"<<Points[ans[i][0]-1]<<
//             //     ans[i][1]<<":"<<Points[ans[i][1]-1]<<
//             //     Points.size()<<":"<<Points[Points.size()-1];
//             break;
//         }
//     }
//     // qDebug()<<"point:"<<Points<<"            pointsize="<<Points.size();
//     // qDebug()<<"新节点："<<m_graph[Points.size()];
//     return Points.size();
// }

int Dijkstra::addPoint(QVector<double> position){
    double t = 15;
    QVector<QVector<int>> ans; //由斜率计算出备选边集
    for(auto it=this->m_graph.begin();it!=m_graph.end();it++){
        QVector<double> p=this->Points[it.key()-1];
        //计算当前位置与当前节点的斜率
        float target=(position[2]-p[2])/(position[0]-p[0]);
        for(auto it1=it.value().begin();it1!=it.value().end();it1++){
            QVector<double> q=this->Points[it1->first-1];
            //计算当前节点与剩余所有节点的斜率
            float k=(q[2]-p[2])/(q[0]-p[0]);
            //若两者斜率小于一定阈值，则将该边加入ans
            if (k-target<1e-2){
                QVector<int> temp;
                temp.push_back(it.key());
                temp.push_back(it1->first);
                ans.append(temp);
            }
        }
    }
    //输出加边信息
    // qDebug()<<"ans:"<<ans;
    // qDebug();

    //从ans中筛选正确的边
    for(int i=0;i<ans.size();i++){
        int res=0;
        if (Points[ans[i][0]-1][0]<=position[0]+t || Points[ans[i][1]-1][0]<=position[0]+t)
            res++;
        if (Points[ans[i][0]-1][0]>=position[0]-t || Points[ans[i][1]-1][0]>=position[0]-t)
            res++;
        if(Points[ans[i][0]-1][2]<=position[2]+t || Points[ans[i][1]-1][2]<=position[2]+t)
            res++;
        if(Points[ans[i][0]-1][2]>=position[2]-t || Points[ans[i][1]-1][2]>=position[2]-t)
            res++;

        //ans[i][0]边的第一个节点，ans[i][1]边的第二个节点
        if (res==4){
            //由确定的边集修正当前点坐标
            DataBase::edge edge = DataBase::getInstance().getEdgeData(ans[i][0],ans[i][1]);
            position=correction(position,edge);
            Points.append(position);
            double d1=qSqrt(pow((position[0]-Points[ans[i][0]-1][0]),2)+pow((position[2]-Points[ans[i][0]-1][2]),2));
            double d2=qSqrt(pow((position[0]-Points[ans[i][1]-1][0]),2)+pow((position[2]-Points[ans[i][1]-1][2]),2));
            //更新第一个点的终点为当前点，权值为d1
            for (int j=0;j<this->m_graph[ans[i][0]].size();j++){
                if(m_graph[ans[i][0]][j].first==ans[i][1]){
                    m_graph[ans[i][0]][j].first=Points.size();
                    m_graph[ans[i][0]][j].second=d1;
                }
            }
            //更新第二个点的终点为当前点，权值为d2
            for (int j=0;j<this->m_graph[ans[i][1]].size();j++){
                if(m_graph[ans[i][1]][j].first==ans[i][0]){
                    m_graph[ans[i][1]][j].first=Points.size();
                    m_graph[ans[i][1]][j].second=d2;
                }
            }

            m_graph[Points.size()].append(qMakePair(ans[i][0], d1));
            m_graph[Points.size()].append(qMakePair(ans[i][1], d2));
            //输出最后选择点以及加的两条边
            // qDebug()<<ans[i][0]<<":"<<Points[ans[i][0]-1]<<
            //     ans[i][1]<<":"<<Points[ans[i][1]-1]<<
            //     Points.size()<<":"<<Points[Points.size()-1];
            break;
        }
    }
    // qDebug()<<"point:"<<Points<<"            pointsize="<<Points.size();
    // qDebug()<<"新节点："<<m_graph[Points.size()];
    return Points.size();
}
/*
 * 修正当前点坐标，使其在路网上
 * position：当前点位置
 * begin：起点编号
 * end：终点编号
*/
QVector<double> Dijkstra::correction(QVector<double> position,DataBase::edge edge){
    double k=edge.k;
    double b=edge.b;
    double frac=1+k*k;

    double x=(position[0]-k*b+k*position[1])/frac;
    double y=(k*position[0]+b+k*k*position[1])/frac;
    return {x,y};

}


/*
 * 迪杰斯特拉算法
 * start：起点编号
 * end：终点编号
*/
Dijkstra::ShortestPathResult Dijkstra::calculateShortestPath(int start, int end) {
    QHash<int, double> dist;  // 节点距离记录
    QHash<int, int> prev;     // 前驱节点记录
    initialize(dist, prev);

    // 有效性检查
    if (!m_graph.contains(start) || !m_graph.contains(end)) {
        return {QList<int>(), 0.0, false};
    }

    // 使用最小堆优化（优先队列存储<距离, 节点>）
    priority_queue<pair<double, int>, vector<pair<double, int>>, greater<>> pq;
    pq.push(make_pair(0.0, start));
    dist[start] = 0.0;

    while (!pq.empty()) {
        auto current = pq.top();
        pq.pop();

        int u = current.second;
        double currentDist = current.first;

        // 跳过非最优路径
        if (currentDist > dist[u]) continue;

        // 遍历邻接节点
        for (const auto& edge : m_graph[u]) {
            int v = edge.first;
            double weight = edge.second;
            double newDist = currentDist + weight;

            // 发现更短路径
            if (newDist < dist[v]) {
                dist[v] = newDist;
                prev[v] = u;
                pq.push(make_pair(newDist, v));
            }
        }
    }
    return buildResult(dist, prev, start, end);
}

void Dijkstra::initialize(QHash<int, double>& dist, QHash<int, int>& prev) const {
    const double INF = numeric_limits<double>::infinity();

    for (auto it = m_graph.begin(); it != m_graph.end(); ++it) {
        int node = it.key();
        dist[node] = INF;
        prev[node] = -1;
    }
}

Dijkstra::ShortestPathResult Dijkstra::buildResult(const QHash<int, double>& dist,
                                                   const QHash<int, int>& prev,
                                                   int start, int end) const {
    ShortestPathResult result;

    // 处理不可达情况
    if (dist[end] == numeric_limits<double>::infinity()) {
        result.isValid = false;
        result.totalDistance = 0.0;
        return result;
    }

    // 回溯路径
    QList<int> path;
    int current = end;
    while (current != start) {
        path.prepend(current);
        current = prev[current];

        // 路径断裂保护
        if (current == -1) {
            return {QList<int>(), 0.0, false};
        }
    }
    path.prepend(start);

    result.path = path;
    result.totalDistance = dist[end];
    result.isValid = true;
    return result;
}
